#define F_CPU 16000000UL

#include <avr/io.h>
#include <stdlib.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <math.h>

#include "led_gen.h"
#include "serial_teste.h"
#include "servo_teste.h"

#define SERIAL_BAUD 57600

static inline void main_init( void );
static inline void reset_buffer(char* msg);

int main (void)
{

  int pos=1500;

  main_init();
  sei();

  while (1) {
    _delay_ms(2);
    if (serial_rx) {
      pos = atoi((const char*) msg_rx);
      if ((pos>=1000) && (pos<=2000)){
	MOVE_SERVO_TO(2*pos);
      }
      serial_transmit((char*) msg_rx);
      rx_buf_pos = 0;
      serial_rx = 0;
    }
  }
  return 0;
}


static inline void main_init( void ) {

  LED1_INIT;
  serial_init((F_CPU/SERIAL_BAUD/16)-1);
  servo_init();

}

/*
  int main (void)
  {
  char buf[50], cur_ch[6];
  int ch;
  
  main_init();
  sei();
  
  while (1) {
    LED1_OFF;
    _delay_ms(500);
    LED1_ON;
    buf = "";
    for (ch=0;ch<CHANNEL_NB;ch++) {
      sprintf(cur_ch,"%d,",serinp[ch]);
      strcat(buf,(const char*) cur_ch);
    }
    sprintf(cur_ch,"\n");
    strcat(buf,(const char*) cur_ch);
    serial_transmit(buf);
  }
  return 0;
}
*/
